import numpy as np
from sympy import *
from OCC.Core.BRepPrimAPI import BRepPrimAPI_MakePrism
from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_MakeEdge, BRepBuilderAPI_MakeWire, BRepBuilderAPI_MakeFace
from OCC.Core.gp import gp_Pnt, gp_Trsf, gp_Vec, gp_Ax1, gp_Dir
import time
import copy

def direction(path):
    end = np.array(path[1])
    start = np.array(path[0])
    dir = end - start
    return dir

def move_path(path,move_dir,distance):
    for j in range(len(path)):
        path[j][0] = (path[j][0] + move_dir * distance)
        path[j][1] = (path[j][1] + move_dir * distance)
    return path

class Line():
    def __init__(self, point, dir):
        self.point = point
        self.dir = dir


class Linear_solver():
    def __init__(self, line0, line1):
        self.line0 = line0
        self.line1 = line1
        self.result = {}

    def solve(self):
        t, q = symbols('t q')
        x0,y0,z0 = tuple(self.line0.point)
        l0,m0,n0 = tuple(self.line0.dir)

        x1,y1,z1 = tuple(self.line1.point)
        l1,m1,n1 = tuple(self.line1.dir)
        result = solve([l0 * t - l1 * q + x0 - x1, m0 * t - m1 * q + y0 - y1, n0 * t - n1 * q + z0 - z1], [t, q])
        self.result = result
        return self.result

    def get_point(self):
        result=self.solve()
        t, q = symbols('t q')
        t = result[t]
        x0,y0,z0 = tuple(self.line0.point)
        l0,m0,n0 = tuple(self.line0.dir)
        x1 = float(x0 + l0*t)
        y1 = float(y0 + m0*t)
        z1 = float(z0 + n0*t)
        point = np.array([x1,y1,z1])
        return point



class PathSplit():
    def __init__(self,path,adjust_times=1):
        self.path = np.array(path)
        self.adjust_times = adjust_times
        self.dir = [(self.path[i][1]-self.path[i][0])for i in range(len(path))]

    @staticmethod
    def coplane(a,b,c):
        d=np.vstack([a, b, c])
        np.linalg.det(d)
        if abs(np.linalg.det(d)) <= 1e-10:
            return True
        else:
            return False
    @staticmethod
    def move(x,dir):
        if isinstance(x,list):
            pass
        elif isinstance(x,np.ndarray):
            for i in range(len(x)):
                x[i] = x[i] + dir
            return x
    @staticmethod
    def merge(path1,path2):
        pt1 = path1[1]
        pt2 = path2[0]
        dif = pt2 - pt1

        new_path1=PathSplit.move(path1,dif/2)
        new_path2=PathSplit.move(path2,-dif/2)
        return [new_path1,new_path2]

    @staticmethod
    def cut_start(path,length):
        dir = path[0][1] - path[0][0]
        dir = dir/np.linalg.norm(dir)
        path[0][0]=path[0][0]+length*dir
        return path
    @staticmethod
    def cut_end(path,length):
        dir = path[-1][1] - path[-1][0]
        dir = dir/np.linalg.norm(dir)
        path[-1][1]=path[-1][1]-length*dir
        return path

    def get_split(self):
        # if len(self.path)<=2:
        #     split = self.path
        # elif len(self.path)>=3:
        #     split = []
        #     split.append([self.path[0].tolist()])
        split=[]
        adjust_times = self.adjust_times
        if adjust_times==1:
            split.append(self.path)
        if adjust_times==2:
            if len(self.path)<=2:
                split.append(self.path.tolist())
            elif len(self.path)>=3:
                split.append([self.path[0].tolist()])

            split[-1].append(self.path[1].tolist())
            split.append([self.path[-2].tolist(),self.path[-1].tolist()])
            split[0]=move_path(split[0],np.array([0,0,-1]),20)

            # split[-1].append(self.path[1].tolist())
            # split.append([self.path[-2].tolist(),self.path[-1].tolist()])
            # split[0]=move_path(split[0],np.array([0,0,-1]),20)
        elif adjust_times==3:
            # split.append(self.path)

            # path1=[self.path[0],self.path[1]]
            # path1=move_path(path1,np.array([1,0,0]),12)
            # # path2=[]
            #
            # path3=[self.path[3],self.path[4]]
            # path3=move_path(path3,np.array([-1,0,0]),12)
            #
            # path2=[self.path[1],self.path[3]]
            # path2=move_path(copy.deepcopy(path2),np.array([0,-1,-1]),20)
            # split.append(path1)
            # split.append(path2)
            # split.append(path3)

            # split.append([self.path[0].tolist()])
            # for k in range(len(self.path)-2):
            #     if PathSplit.coplane(self.dir[k],self.dir[k+1],self.dir[k+2]):
            #         # if len(self.dir[k+2])<=20:
            #         #     split[-1].extend([self.path[k + 1].tolist(), self.path[k + 2].tolist()])
            #         # else:
            #         split[-1].extend([self.path[k+1].tolist(),self.path[k+2].tolist()])
            #     else:
            #         split[-1].append(self.path[k+1].tolist())
            #         split.append([self.path[k+1].tolist()])
            # split[-1].append(self.path[-1].tolist())

            path1 = [self.path[0],self.path[1]]
            path3 = [self.path[-2],self.path[-1]]

            path2 = PathSplit.merge(copy.deepcopy(self.path[1]),copy.deepcopy(self.path[-2]))
            path1 = move_path(path1,np.array([-1,0,0]),10)
            path2 = PathSplit.cut_start(path2,10)
            path2 = PathSplit.cut_end(path2,10)
            path3 = move_path(path3,np.array([1,0,0]),10)
            split.append(path1)
            split.append(path2)
            split.append(path3)

            # for k in range(len(self.path)-1):
            #     split.append([self.path[k],self.path[k+1]])
            #
            # split[1]=move_path(copy.deepcopy(split[1]),np.array([-1,0,1/2]),20)
            # split[2]=move_path(copy.deepcopy(split[2]),np.array([0,0,1]),20)


            # split[0]=move_path(split[0],np.array([0,0,-1]),20)
            # split[1]=move_path(split[1],np.array([1,0,0]),20)
                # split[2]=move_path(split[2],np.array([0,1,0]),20)


            # i = 0
            # while i < len(self.path)-2:
            #     if PathSplit.coplane(self.dir[i],self.dir[i+1],self.dir[i+2]):
            #         split[-1].extend([self.path[i+1].tolist(),self.path[i+2].tolist()])
            #     else:
            #         split[-1].append(self.path[i+1].tolist())
            #         split.append([self.path[i+2].tolist()])
            #     i=i+2
            # if i==len(self.path)-1:
            #     pass
            # else:
            #     split[-1].append(self.path[-1].tolist())
        return split


class PathPrism():
    def __init__(self,path,dx,dy=None,dz=None):
        self.path = np.array(path)
        self.dx = dx
        self.dirs = []
        self.unit_verticals=[]
        self.unit_normal = None
        self.positive_points = []
        self.negative_points = []
        self.positive_lines = []
        self.negative_lines = []
        self.positive_cross_points = []
        self.negative_cross_points = []

    def init(self):
        if len(self.path)>1:
            edge_0 = self.path[0]
            edge_1 = self.path[1]
            dir0 = edge_0[1] - edge_0[0]
            dir1 = edge_1[1] - edge_1[0]
            normal = np.cross(dir0, dir1)
            self.unit_normal = normal / np.linalg.norm(normal)
            for i in range(len(self.path)):
                dir = self.path[i][1] - self.path[i][0]
                self.dirs.append(dir)
                unit_dir = dir / np.linalg.norm(dir)
                unit_vertical = np.cross(self.unit_normal, unit_dir)
                self.unit_verticals.append(unit_vertical)
                pos_pnt = self.path[i][0] + unit_vertical * self.dx / 2
                neg_pnt = self.path[i][0] - unit_vertical * self.dx / 2
                self.positive_points.append(pos_pnt)
                self.negative_points.append(neg_pnt)
                pos_line = Line(pos_pnt, dir)
                neg_line = Line(neg_pnt, dir)

                self.positive_lines.append(pos_line)
                self.negative_lines.append(neg_line)

                if i == len(self.path) - 1:
                    self.positive_points.append(self.path[i][1] + unit_vertical * self.dx / 2)
                    self.negative_points.append(self.path[i][1] - unit_vertical * self.dx / 2)

        elif len(self.path) == 1:
            dir0 = self.path[0][1] - self.path[0][0]
            #self.dirs.append(dir0)
            dir1 = np.array([0.0,0.0,1.0])
            normal = np.cross(dir0,dir1)
            if all(normal==np.array([0,0,0])):
                self.unit_normal = np.array([0.0,1.0,0.0])
            else:
                self.unit_normal = normal / np.linalg.norm(normal)
            unit_dir = dir0 / np.linalg.norm(dir0)
            self.dirs.append(unit_dir)
            unit_vertical = np.cross(self.unit_normal, unit_dir)
            self.unit_verticals.append(unit_vertical)


            self.positive_cross_points.append(self.path[0][0] + unit_vertical * self.dx / 2)
            self.positive_cross_points.append(self.path[0][1] + unit_vertical * self.dx / 2)
            self.negative_cross_points.append(self.path[0][0] - unit_vertical * self.dx / 2)
            self.negative_cross_points.append(self.path[0][1] - unit_vertical * self.dx / 2)

    def get_dirs(self):
        return self.dirs

    def get_unit_verticals(self):
        return self.unit_verticals

    def get_pos_points(self):
        return self.positive_points

    def get_neg_points(self):
        return self.negative_points

    def get_pos_lines(self):
        return self.positive_lines

    def get_neg_lines(self):
        return self.negative_lines

    def get_pos_cross_points(self):
        self.positive_cross_points.append(self.get_pos_points()[0])
        for i in range(len(self.path)-1):
            Ls = Linear_solver(self.get_pos_lines()[i],self.get_pos_lines()[i+1])
            point = Ls.get_point()
            self.positive_cross_points.append(point)
        self.positive_cross_points.append(self.get_pos_points()[-1])
        return self.positive_cross_points

    def get_neg_cross_points(self):
        self.negative_cross_points.append(self.get_neg_points()[0])
        for i in range(len(self.path)-1):
            Ls = Linear_solver(self.get_neg_lines()[i],self.get_neg_lines()[i+1])
            point = Ls.get_point()
            self.negative_cross_points.append(point)
        self.negative_cross_points.append(self.get_neg_points()[-1])
        return self.negative_cross_points

    def cycle(self):
        if len(self.path)>1:
            pos = self.get_pos_cross_points()
            neg = self.get_neg_cross_points()[::-1]
            return np.array(pos+neg)

        elif len(self.path)==1:
            pos = self.positive_cross_points
            neg = self.negative_cross_points[::-1]
            return np.array(pos+neg)

class PrismBuilder():
    def __init__(self,cycle,unit_normal,h):
        self.cycle = cycle
        self.mkw = BRepBuilderAPI_MakeWire()
        self.unit_normal = unit_normal
        self.h = h
        self.edges = []

    def build_edges(self):
        for i in range(len(self.cycle)):
            if i<=len(self.cycle)-2:
                start = gp_Pnt(self.cycle[i][0],self.cycle[i][1],self.cycle[i][2])
                end = gp_Pnt(self.cycle[i+1][0],self.cycle[i+1][1],self.cycle[i+1][2])
                self.edges.append(BRepBuilderAPI_MakeEdge(start, end).Edge())
            if i==len(self.cycle)-1:
                start = gp_Pnt(self.cycle[i][0],self.cycle[i][1],self.cycle[i][2])
                end = gp_Pnt(self.cycle[0][0],self.cycle[0][1],self.cycle[0][2])
                self.edges.append(BRepBuilderAPI_MakeEdge(start, end).Edge())

    def add_edges(self):
        for i in range(len(self.edges)):
            self.mkw.Add(self.edges[i])

    def build_prism(self):
        self.build_edges()
        self.add_edges()
        dire = self.h * self.unit_normal
        direction = gp_Vec(dire[0], dire[1], dire[2])
        face = BRepBuilderAPI_MakeFace(self.mkw.Wire()).Face()
        prism=BRepPrimAPI_MakePrism(face,direction).Shape()
        return prism


class PathCalibrator():
    def __init__(self, path, cali_coefficient):
        self.path = path
        self.cali_coefficient = cali_coefficient

    @staticmethod
    def projection_distance(point1, point2):
        vector1 = np.array(point1[:2])
        vector2 = np.array(point2[:2])
        distance = np.linalg.norm(vector1 - vector2)
        return distance

    def calibration(self):
        if len(self.path)>=3:
            v_last=self.path[-1][1]-self.path[-1][0]
            dis=np.linalg.norm(v_last)
            if dis<=self.cali_coefficient:
                self.path = np.delete(self.path, -1,axis=0)
                self.path[-1][0]=self.path[-1][0]+v_last
                self.path[-1][1]=self.path[-1][1]+v_last
                self.path[-2][1]=self.path[-2][1]+v_last
            else:
                pass
        elif len(self.path)==2:
            v_last=self.path[-1][1]-self.path[-1][0]
            dis=np.linalg.norm(v_last)
            if dis<=self.cali_coefficient:
                self.path = np.delete(self.path, -1, axis=0)
    def to_new_path(self):
        self.calibration()
        return self.path

#path = [[[0,0,0] , [0,0,100]], [[0,0,100] ,[0,100,100]],[[0,100,100],[0,100,300]],[[0,100,300],[0,200,400]],[[0,200,400],[0,-100,500]]]

#path = [[[1619.91, 724.912, -646.627], [1613.91, 724.912, -646.627]], [[1613.91, 724.912, -646.627], [1613.91, 724.912, -664.627]], [[1613.91, 724.912, -664.627], [1610.91, 724.912, -664.627]]]

if __name__ == '__main__':
    import trimesh
    # path = [ [[0,0,0] , [0,0,100]],[[0,0,100],[0,50,150]]]
    path = [[[0,0,0] , [0,0,100]], [[0,0,100] ,[0,100,100]],[[0,100,100],[0,100,300]],[[0,100,300],[0,200,400]],[[0,200,400],[0,-100,500]]]
    # path =[
    #         [[2346.62 , 645.14 , 635.04], [2474.62 , 645.14 , 635.04]],
    #         [[2474.62 , 645.14 , 635.04], [2474.62 , 649.14 , 635.04]],
    #         [[2474.62 , 649.14 , 635.04], [2476.62 , 649.14 , 635.04]]
    #     ]
    path2=[
        [[2471.85 , 751.26 , 637.7],[2471.85 , 751.26 , 629.7]],
        [[2471.85 , 751.26 , 629.7],[2471.85 , 761.26 , 629.7]],
        [[2471.85 , 751.26 , 629.7],[2471.85 , 751.26 , 600.7]],

    ]

    path3=[
        [[2471.85 , 751.26 , 637.7],[2471.85 , 751.26 , 629.7]],
        [[2471.85 , 751.26 , 629.7],[2471.85 , 761.26 , 629.7]],
        [[2471.85 , 761.26 , 629.7],[2471.85 , 761.26 , 627.7]],
        [[2471.85 , 761.26 , 627.7],[2471.85 , 701.26 , 627.7]]
        ]

    path3D=[
        [[2416.62 , 742.64 , 704.04],[2420.62 , 742.64 , 704.04]],
        [[2420.62 , 742.64 , 704.04],[2420.62 , 756.64 , 704.04]],
        [[2420.62 , 756.64 , 704.04],[2420.62 , 756.64 , 662.04]],
        [[2420.62 , 756.64 , 662.04],[2540.62 , 756.64 , 662.04]],
        [[2540.62 , 756.64 , 662.04],[2540.62 , 776.64 , 662.04]],
        [[2540.62 , 776.64 , 662.04],[2540.62 , 776.64 , 660.04]]
        ]

    path4 = [
        [[0,0,0],[30,0,0]],
        [[30,0,0],[30,30,0]],
        [[30,30,0],[0,30,0]]
    ]

    path_cut1=[[[-114.2197937193761, 773.03076, 2081.786], [-114.2197937193761, 769.03076, 2033.786]], [[-114.2197937193761, 769.03076, 2033.786], [-183.2197937193761, 769.0307599999998, 2033.786]]]
    # np.set_printoptions(precision=3)
    path = np.array(path4)

    path=np.round(path,decimals=2)

    dif_path = [(path[i][1]-path[i][0])for i in range(len(path))]

    np.set_printoptions(suppress=True)
    print(dif_path)
    Cali = PathCalibrator(path,2)
    path_n = Cali.to_new_path()
    path_t = trimesh.load_path(path)
    path_new = trimesh.load_path(path_n)
    # maze.get_pointcloud().show()
    scene = trimesh.Scene([path_new])
    scene.show()


    pp=PathSplit(path)
    # print(pp.get_split())
    compound=[]
    for path in pp.get_split():
        # print(path)
        # temp = trimesh.load_path(path)
        # scene=trimesh.Scene([temp])
        # scene.show()

        Ps = PathPrism(path,dx=5)
        Ps.init()
        cycles = Ps.cycle()
        print(cycles)
        PB = PrismBuilder(cycles,Ps.unit_normal,2)
        prism=PB.build_prism()
        compound.append(prism)
    from OCC.Display.SimpleGui import init_display

    display, start_display, add_menu, add_function_to_menu = init_display("wx")

    display.DisplayShape(compound, update=True)
    start_display()